// --------------------------------------------------------------------------------------------------------------------
// <copyright file="GlobalMapModelAgent.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Navigation.Visualization
{
    using System;
    using System.Linq;
    using System.Runtime.Serialization;

    using Microsoft.Robotics.Hardware.DAC;
    using Microsoft.Robotics.Navigation.Perception;
    using Microsoft.Robotics.Navigation.Runtime.Localization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Visualization;
         
    /// <summary>
    /// Generates a model for visualization of the global map.
    /// </summary>
    /// <typeparam name="THorizontalDepthProfile">The kind of horizontal depth profile to display</typeparam>
    [DataContract]
    public sealed class GlobalMapModelAgent<THorizontalDepthProfile> :
        ModelAgentBase<MessageTuple<SimpleAgentMessage<THorizontalDepthProfile>, SLAM2DAgentMessage>, GlobalMapModel>,
        ISubscriptionReceiver<MessageTuple<SimpleAgentMessage<THorizontalDepthProfile>, SLAM2DAgentMessage>>
        where THorizontalDepthProfile : IHorizontalDepthProfile
    {
        /// <summary>
        /// The name of the mapping agent we subscribe to.
        /// </summary>
        [DataMember(Name = "MapProducer")]
        private readonly string mapProducer;

        /// <summary>
        /// The name of the horizontal depth profile agent we subscribe to
        /// </summary>
        [DataMember(Name = "HorizontalDepthProfileProducer")]
        private readonly string hdpProducer;

        /// <summary>
        /// The depth cutoff distance to use when visualizing the horizontal depth profile
        /// </summary>
        [DataMember(Name = "MaximumDepth")]
        private readonly double maximumDepth;

        /// <summary>
        /// Name of the localization agent
        /// </summary>
        [DataMember(Name = "ClickToLocalizeReceiver")]
        private readonly string clickToLocalizeReceiverName;

        /// <summary>
        /// game pad message forwarder.
        /// </summary>
        private IForwarder<SimpleAgentMessage<Pose2D>> clickToLocalizeForwarder;

        /// <summary>
        /// Global map file.
        /// </summary>
        [DataMember]
        private string mapFile;

        /// <summary>
        /// The synchronization agent used to sync the map with the hdp.
        /// </summary>
        private IAgentHost syncAgent;

        /// <summary>
        /// Initializes a new instance of the GlobalMapModelAgent class.
        /// </summary>
        /// <param name="name">The name of the agent.</param>
        /// <param name="mapProducer">The SLAM producer agent.</param>        
        /// <param name="hdpProducer">The horizontal depth profile producer.</param>   
        /// <param name="maximumDepth">The depth cutoff for depth profile rendering</param>
        /// <param name="maxUpdateIntervalMilliseconds">The max UI update interval.</param>
        /// <param name="mapFile">Global map file</param>
        /// <param name="clickToLocalizeReceiver">The localization agent</param>
        public GlobalMapModelAgent(
            string name,
            IProducer<SLAM2DAgentMessage> mapProducer,
            IProducer<SimpleAgentMessage<THorizontalDepthProfile>> hdpProducer,
            double maximumDepth,
            int maxUpdateIntervalMilliseconds,
            string mapFile = null,
            IControlReceiver<SimpleAgentMessage<Pose2D>> clickToLocalizeReceiver = null)
            : base(name, maxUpdateIntervalMilliseconds)
        {
            this.mapFile = mapFile;
            this.hdpProducer = hdpProducer.Name;
            this.maximumDepth = maximumDepth;
            this.mapProducer = mapProducer.Name;
            if (clickToLocalizeReceiver != null)
            {
                this.clickToLocalizeReceiverName = clickToLocalizeReceiver.Name;
            }
        }

        /// <summary>
        /// Initialize the agent. 
        /// </summary>
        /// <param name="locator">The locator to use.</param>
        public override void Initialize(AgentLocator locator)
        {
            base.Initialize(locator);

            this.Model.InitializeWithMap(this.mapFile);
            this.syncAgent = locator.JoinSubscribe(this.hdpProducer, this.mapProducer, this, 1, false, true);

            if (this.clickToLocalizeReceiverName != null)
            {
                IAgentHost localizationAgent = locator.GetAgent(this.clickToLocalizeReceiverName);
                this.clickToLocalizeForwarder = localizationAgent.GetControlPort<SimpleAgentMessage<Pose2D>>();
                this.Model.ClickToLocalizeDelegate = (pose) => this.clickToLocalizeForwarder.Post(new SimpleAgentMessage<Pose2D>(pose, AgentMessage.GetCurrentTime()));
            }
            else
            {
                this.Model.ClickToLocalizeDelegate = (message) => { };
            }
        }
            
        /// <summary>
        /// Activates the sync agent
        /// </summary>
        public override void OnActivated()
            {
            base.OnActivated();
            this.syncAgent.Activate();
            }
        
        /// <summary>
        /// deactivates the sync agent
        /// </summary>
        public override void OnDeactivated()
        {
            base.OnDeactivated();
            this.syncAgent.Deactivate();
        }

        /// <summary>
        /// Update the model when a new map is received
        /// </summary>
        /// <param name="messageTuple">State containing a local map</param>
        /// <returns>True if the model was updated</returns>
        protected override bool UpdateModel(MessageTuple<SimpleAgentMessage<THorizontalDepthProfile>, SLAM2DAgentMessage> messageTuple)
        {
            SLAM2DAgentMessage mapMessage = messageTuple.Message2;
            THorizontalDepthProfile hdp = messageTuple.Message1.State;
            Pose2D mostLikelyPose = mapMessage.Map.MapStartPose;
            
            if (mapMessage.Particles != null)
            {
                this.Model.UpdateFromParticles(mapMessage.Particles, hdp, this.maximumDepth);
                mostLikelyPose = mapMessage.Particles[this.Model.SelectedParticleId - 1].Pose;
                }
                else
                {
                this.Model.UpdateFromMap(mapMessage.Map);
                mostLikelyPose = this.Model.GetMapPoseInGlobalFrame(mapMessage.Location);
                this.Model.UpdateFromPoses(mostLikelyPose, mapMessage.Poses.Select(p => this.Model.GetMapPoseInGlobalFrame(p)));
                this.Model.UpdateFromHdp(hdp, mostLikelyPose, this.maximumDepth);
                }

            this.Model.UpdatePath(mostLikelyPose);
            this.Model.UpdateRobotPose(mostLikelyPose);
            return true;
        }
    }
}
